#include "mainwindow.h"
#include "ui_mainwindow.h"

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    initGUI();
    initInstances();
}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::on_btnQuit_clicked()
{
    interpolater->stop();
    qApp->quit();
}

void MainWindow::initGUI()
{
    this->setFocus();
    QRegExpValidator* dValidator = new QRegExpValidator(QRegExp("[-+]?\\d{0,4}\\.?(\\d{0,3})?"));
    QIntValidator* iValidator = new QIntValidator(1, 99999);
    ui->editX1->setValidator(dValidator);
    ui->editY1->setValidator(dValidator);
    ui->editX2->setValidator(dValidator);
    ui->editY2->setValidator(dValidator);
    ui->editCycleTime->setValidator(iValidator);
    ui->editFeedRate->setValidator(iValidator);
    ui->editADTime->setValidator(iValidator);
//    ui->editFIROrder->setValidator(iValidator);
    ui->lblSim->hide();
    ui->btnStopSim->hide();

    ui->widgetSpeedX->setAxisNames("Vx(mm/s)", "t");
    ui->widgetSpeedY->setAxisNames("Vy(mm/s)", "t");
}

void MainWindow::initInstances()
{
    basicMotionPath = new MotionPath;
    interpreter = new GInterpreter;
    interpolater = new Interpolater;
    connect(interpolater, SIGNAL(output(double,double)),
            this, SLOT(smooth_and_feed(double,double)));
    filterX = new FIRFilter;
    filterY = new FIRFilter;
}

void MainWindow::on_btnSim_clicked()
{
    ui->lblSim->show();
    ui->btnStopSim->show();
    QString gcode;
    gcode.append(ui->cbbGCode->currentText())
            .append("X").append(ui->editX1->text())
            .append("Y").append(ui->editY1->text()).append("\n")
            .append(ui->cbbGCode2->currentText())
            .append("X").append(ui->editX2->text())
            .append("Y").append(ui->editY2->text());
//    qDebug() << "gcode:" << gcode;
    int ret = interpreter->interpret(gcode, basicMotionPath);
//    qDebug() << "nodes:" << basicMotionPath->getMotionPath().size();
    if (ret) return;
    ui->trackWidget->drawPath(basicMotionPath->getMotionPath(),
                              basicMotionPath->getPathGeometry());
//    qDebug() << basicMotionPath->getPathGeometry().minX
//             << basicMotionPath->getPathGeometry().maxX
//             << basicMotionPath->getPathGeometry().minY
//             << basicMotionPath->getPathGeometry().maxY;
    int feedrate = ui->editFeedRate->text().toInt();
    int cycleTime = ui->editCycleTime->text().toInt();
    double wholeLength = basicMotionPath->getWholeLength();
    ui->widgetSpeedX->setAxisRanges(-feedrate / 60.0, feedrate / 60.0,
                                    0,wholeLength / feedrate * 60);
    ui->widgetSpeedY->setAxisRanges(-feedrate / 60.0, feedrate / 60.0,
                                    0, wholeLength / feedrate * 60);
    ui->widgetSpeedX->reset();
    ui->widgetSpeedY->reset();
    currentX = currentY = 0.0;
    outputCount = 0;
    interpolater->setFeedRate(feedrate);
    interpolater->setPeriod(cycleTime);
    if (ui->chbUseFIR->isChecked())
    {
        int AD_period = ui->editADTime->text().toInt();
        filterX->reset();
        filterY->reset();
        filterX->setPeriod(AD_period);
        filterY->setPeriod(AD_period);
        zeroOutputNum = 0;
        interpolater->setKeepRunning();
        ui->widgetSpeedX->setAxisRanges(-feedrate / 60.0, feedrate / 60.0,
                                        0,wholeLength / feedrate * 60 +
                                        cycleTime * AD_period / 1000.0);
        ui->widgetSpeedY->setAxisRanges(-feedrate / 60.0, feedrate / 60.0,
                                        0, wholeLength / feedrate * 60 +
                                        cycleTime * AD_period / 1000.0);
        /**
         * @brief maxError 理论最大轮廓误差
         * @ref 参考论文：《High speed cornering strategy with confined contour
         *               error and vibration suppression for CNC machine tools》
         */
        double f = feedrate * 1000.0 / 60.0; // feedrate in um/s
        double t = cycleTime * AD_period / 1000.0; // Acc/Dec time in s
        // vector for two blocks
        QPointF v0(basicMotionPath->getMotionPath().at(0)->getEndPoint().x(),
                   basicMotionPath->getMotionPath().at(0)->getEndPoint().y());
        QPointF v1(basicMotionPath->getMotionPath().at(1)->getEndPoint().x(),
                   basicMotionPath->getMotionPath().at(1)->getEndPoint().y());
        double beta = atan2(v1.y(), v1.x()) - atan2(v0.y(), v0.x()); // cornering angle
        if (beta < -M_PI) beta += M_PI * 2;
        else if (beta > M_PI) beta = M_PI * 2 - beta;
        beta = fabs(beta);
        double maxError = sqrt(f * f * t * t * (1 - cos(beta)) / 32.0);
        ui->lblCmdError->setText(tr("拐角：%1 rad（%2°）， 理论最大轮廓误差：%3 μm。")
                                 .arg(beta).arg(beta / M_PI * 180).arg(maxError));
    }
    else
    {
        // vector for two blocks
        QPointF v0(basicMotionPath->getMotionPath().at(0)->getEndPoint().x(),
                   basicMotionPath->getMotionPath().at(0)->getEndPoint().y());
        QPointF v1(basicMotionPath->getMotionPath().at(1)->getEndPoint().x(),
                   basicMotionPath->getMotionPath().at(1)->getEndPoint().y());
        double beta = atan2(v1.y(), v1.x()) - atan2(v0.y(), v0.x()); // cornering angle
        if (beta < -M_PI) beta += M_PI * 2;
        else if (beta > M_PI) beta = M_PI * 2 - beta;
        beta = fabs(beta);
        ui->lblCmdError->setText(tr("拐角：%1 rad（%2°）， 理论最大轮廓误差：0 μm。")
                                 .arg(beta).arg(beta / M_PI * 180));
    }
    interpolater->interpolate(basicMotionPath->getMotionPath());
    ui->lblSim->hide();
    ui->btnStopSim->hide();
}

void MainWindow::smooth_and_feed(double incremX, double incremY)
{
    if (ui->chbUseFIR->isChecked())
    {
//        qDebug() << "+ dx:" << incremX << "dy:" << incremY;
        filterX->setInput(incremX);
        incremX = filterX->getOutput();
        filterY->setInput(incremY);
        incremY = filterY->getOutput();
        if (fabs(incremX) < 1e-9 && fabs(incremY) < 1e-9)
        {
            zeroOutputNum++;
            if (zeroOutputNum > 10)
            {
                interpolater->stop();
                zeroOutputNum = 0;
                return;
            }
        }
        else
        {
            zeroOutputNum = 0;
        }
    }
//    qDebug() << "  dx:" << incremX << "dy:" << incremY;
    if (fabs(incremX) < 1e-9) incremX = 0.0;
    if (fabs(incremY) < 1e-9) incremY = 0.0;
    currentX += incremX;
    currentY += incremY;
    ui->trackWidget->addInterpPoint(new QPointF(currentX, currentY));
    int cycle = ui->editCycleTime->text().toInt();
    ui->widgetSpeedX->addPoint(new QPointF(outputCount * cycle / 1000.0,
                                           incremX / cycle * 1000));
    ui->widgetSpeedY->addPoint(new QPointF(outputCount * cycle / 1000.0,
                                           incremY / cycle * 1000));
    outputCount++;
    qApp->processEvents();
//    qDebug() << "dx:" << incremX << "dy:" << incremY
//             << "X: " << currentX << "Y: " << currentY << outputCount;
}

void MainWindow::on_btnStopSim_clicked()
{
    interpolater->stop();
    ui->btnSim->setFocus();
}
